/**
 * @author Yunfeng Bai
 *
 * 2010-11-14
 */
package simulation;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;

import rigidbody.RigidBody;

import collision.Collision;
import force.SpringPenaltyForce;

public class SpringConstraintSolver implements ConstraintSolver {
	private Simulator simulator;
	private HashSet<SpringPenaltyForce> springPairs = new HashSet<SpringPenaltyForce>();
	protected CollisionDetector detector = new CollisionDetector();
	
	/**
	 * This method use penalty method to solve constraints.
	 * 
	 * 1. Check every pair of RigidBody if they penetrate at current time step,
	 * 	  if they are not, disconnect the spring.
	 * 2. For every collision at this point, if there is no spring between the two RigidBody's,
	 *    connect them with a spring.
	 * 
	 * @param collisions_
	 */
	public boolean solveVelocities(Simulator sim)
	{
		// (TODO) more accurate penalty force.
		simulator = sim;
		List<Collision> collisions = detector.detect(sim.getBodies());
		
		List<SpringPenaltyForce> deletingSprings = new ArrayList<SpringPenaltyForce>();
		for (SpringPenaltyForce spring : springPairs)
		{
			boolean found = false;
			for (Collision c : collisions)
			{
				if ((c.object1 == spring._body1 && c.object2 == spring._body2)
					|| (c.object2 == spring._body1 && c.object1 == spring._body2))
				{
					found = true;
					break;
				}
			}
			if (found == false)
			{
				deletingSprings.add(spring);
				if (spring._type == 0)
				{
					simulator.removePersistantForce(spring);
				}
				else
				{
					// Energy reduce.
					spring._body1.getAngularMomentum().scale(0.9);
					spring._body1.getLinearMomentum().scale(0.9);
					spring._body2.getAngularMomentum().scale(0.9);
					spring._body2.getLinearMomentum().scale(0.9);
				}
			}
		}
		for (SpringPenaltyForce spring : deletingSprings) springPairs.remove(spring);
		
		for (Collision c : collisions)
		{
			if (c.type == 0)
			{
				SpringPenaltyForce spring =
					new SpringPenaltyForce(c.object1, c.object2, c.positionInWorld);
				if (springPairs.contains(spring) == false)
				{
					springPairs.add(spring);
					simulator.addPersistantForce(spring);
				}
			}
			else if (c.type == 1)
			{
				SpringPenaltyForce spring =
					new SpringPenaltyForce(c.object1, c.object2, c.positionInWorld, c.penetrationDepth, c.normal);
				simulator.addTempForce(spring);
				springPairs.add(spring);
			}
		}
		
		return false;
	}
	
	public void solveContacts(Simulator sim)
	{
	}

	@Override
	public void rigidBodyAdded(Simulator sim)
	{
	}
}
